#include "SimpleFOC.h"
#include "MotorHall.h"
#include "HalPWM.h"
#include "encode.h"
Simple_HallSensor_t  hall_sensor;
SimpleEncode_t       inc_encode;

Encode_Def_t         encode_inc;

BLDCMotor_t     simple_motor;
SimpleSensor_t  foc_encode;
BLDCDriver_t    bldc_driver;
/***********************************/
//用户增量编码器
void UserInc_update(void)
{
	
}
float UserInc_getAngle(void)
{
	return SimpleEncode_getAngle(&inc_encode);
}
float UserInc_getSensorAngle(void)
{
  return SimpleEncode_getSensorAngle(&inc_encode);
}
float UserInc_getMechanicalAngle(void)
{
	return SimpleEncode_getMechanicalAngle(&inc_encode);
}
double UserInc_getPreciseAngle()
{
	return SimpleEncode_getPreciseAngle(&inc_encode);
}
int UserInc_getFullRotations()
{
	return SimpleEncode_getFullRotations(&inc_encode);
}
/****************************************/

/*************************************************/
//用户霍尔传感器
void UserHall_update(uint8_t state)
{
	HallSensor_updateState(&hall_sensor,state);
}
float UserHall_getAngle(void)
{
	return HallSensor_getAngle(&hall_sensor);
}
float UserHall_getSensorAngle(void)
{
  return HallSensor_getSensorAngle(&hall_sensor);
}
float UserHall_getMechanicalAngle(void)
{
	return HallSensor_getMechanicalAngle(&hall_sensor);
}

double UserHall_getPreciseAngle(void)
{
	return HallSensor_getPreciseAngle(&hall_sensor);
}

int UserHall_getFullRotations(void)
{
	return HallSensor_getFullRotations(&hall_sensor);
}
float UserHall_GetVelocity(void)
{
	return HallSensor_getVelocity(&hall_sensor);
	//EncodeDifference(encode_inc.value.encode_value,encode_inc.last_value,encode_inc);
	//int err=EncodeDifference(encode_inc.value.encode_value,encode_inc.last_value,encode_inc);
	//return (err/0.001)/360.0/60.0;
}
float rmd=0.0;
void update_EncodeSpeed(void)
{
	Read_Encode(&encode_inc);
	int err=EncodeDifference(encode_inc.value.encode_value,encode_inc.last_value,encode_inc);
	encode_inc.last_value=encode_inc.value.encode_value;
	rmd=((err/0.001)/4096.0)*60.0;
}
/*************************************************/

void Init_Simple(void)
{
	BLDCMotor_Init(&simple_motor,4,NOT_SET,NOT_SET,NOT_SET);
	SimpleTime_Init(GetTimeValue,65535);
	Start_UserTime();
	foc_encode.function.getAngle=UserHall_getAngle;
	foc_encode.function.getFullRotations=UserHall_getFullRotations;
	foc_encode.function.getMechanicalAngle=UserHall_getMechanicalAngle;
	foc_encode.function.getPreciseAngle=UserHall_getPreciseAngle;
	foc_encode.function.getSensorAngle=UserHall_getSensorAngle;
	foc_encode.function.getVelocity=UserHall_GetVelocity;
	
	hall_sensor.velocity_max=10000;
	
	bldc_driver.function.setPwm=BLDCDriver6PWM_setPwm;
	bldc_driver.function.setPhaseState=BLDCDriver6PWM_SetPhaseState;
	bldc_driver.function.disable=BLDCDriver6PWM_disable;
	bldc_driver.function.enable= BLDCDriver6PWM_enable;
	simple_motor.run_config.torque_controller=TorqueControl_voltage;      //力矩控制模式
	simple_motor.run_config.controller=MotionControl_velocity;            //速度闭环
	simple_motor.run_config.foc_modulation=FOCModulation_SinePWM;  //空间PWM
	
	simple_motor.PID_velocity.P=0.02f;
	simple_motor.PID_velocity.I=0.1f;
	simple_motor.PID_velocity.D=0.0f;
	
	simple_motor.LPF_velocity.Tf=0.01f;
	
	hall_sensor.getHallState=Get_Hall_State;
	HallSensor_init(&hall_sensor,4);
	
	
	bldc_driver.voltage_power_supply=24;
	simple_motor.run_config.voltage_sensor_align=1;
	BLDCDriver_Init(&bldc_driver);
	
	simple_motor.motor_limit.voltage_limit=13.5;
	
	linkDriver(&simple_motor,&bldc_driver);
	linkSensor(&simple_motor,&foc_encode);
	simple_motor.run_state.voltage.q=0;
	simple_motor.enabled=1;
	simple_motor.sensor_config.sensor_direction= Direction_CW;
	simple_motor.sensor_config.zero_electric_angle=0;
	
	Encoder_Function(&encode_inc,Inc_Encode_Read);   //注册编码器读数
  Encoder_Registration(&encode_inc,0,4095,12);     //注册编码器
	//simple_motor.sensor_config.sensor_direction= NOT_SET;
	Simple_init(&simple_motor);
	//alignSensor(&simple_motor);
}
float f_speed=0.0;
uint8_t c_i=0;
/*********************************
* 循环
*********************************/
void simfoc_loop(void)
{
	//simple_loopFOC(&simple_motor);
	c_i++;
	if(c_i==10)
	{
		c_i=0;
		update_EncodeSpeed();
		//simple_move(&simple_motor,f_speed);
	}
}